# -*- coding: utf-8 -*-
'''
Created on 21.11.2019

@author: yu03
'''

from FFT_Interpolation import *
from mpl_toolkits.mplot3d import Axes3D
from scipy import signal
from scipy.optimize import curve_fit
from scipy.signal import *
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from New_Model.Simulation_File import path, np_16line_name
import time

fig = plt.figure()

j = complex(0, 1)
c = 3e8 # 光速 [m/s]
Lamda = 633e-9 # 光波长 [m]
Fc = c / Lamda # 光频率 [Hz]

pix_size = 5.3e-6
pix_num = 1280
screen_diameter = pix_num * pix_size


dx = np.linspace(0, (pix_num-1)*pix_size, num=pix_num)
dy = dx
V_r_x, V_r_y, V_r_z = 0.0015, 0.0015, 1 # Reference vector
L, D = 0.1, 0.1  # D = displacement, L is static
V_m_x, V_m_y, V_z_m = 0.000, 0.000, 1 # Measurement vector

X, Y = np.meshgrid(dx, dy)

hor_lines = np.linspace(640-7,640+8,16,dtype='int')

def f(X, Y):
    '''
        Window 2d
    '''
    window = signal.gaussian(pix_num, std=pix_num/10)
    window_x, window_y = np.meshgrid(window, window)
    window_2d = window_x*window_y
    
    k = 2 * np.pi / Lamda
    I_0 = 500
    
    '''
        Wave Generating
    '''
    diff_Z_p = D * 2 / (1-V_m_x**2-V_m_y**2) + L * 2 * ((V_m_x**2+V_m_y**2)-(V_r_x**2+V_r_y**2)) / (1-V_m_x**2-V_m_y**2) / (1-V_r_x**2-V_r_y**2)
#     d_ref = X*2*V_r_x*(1-V_r_x**2-V_r_y**2)/(1-V_r_x**2)/(1+V_r_x**2+V_r_y**2) + Y*2*V_r_y*(1-V_r_x**2-V_r_y**2)/(1-V_r_y**2)/(1+V_r_x**2+V_r_y**2) - L*4*(V_r_x**2+V_r_y**2)/(1-(V_r_x**2+V_r_y**2)**2)
#     d_mea = X*2*V_m_x*(1-V_m_x**2-V_m_y**2)/(1-V_m_x**2)/(1+V_m_x**2+V_m_y**2) + Y*2*V_m_y*(1-V_m_x**2-V_m_y**2)/(1-V_m_y**2)/(1+V_m_x**2+V_m_y**2) - (L+D)*4*(V_m_x**2+V_m_y**2)/(1-(V_m_x**2+V_m_y**2)**2)
    d_mea = (X*2*V_m_x+Y*2*V_m_y)/(1+V_m_x**2+V_m_y**2) - (L+D)*4*(V_m_x**2+V_m_y**2)/(1-(V_m_x**2+V_m_y**2)**2)
    d_ref = (X*2*V_r_x+Y*2*V_r_y)/(1+V_r_x**2+V_r_y**2) - L*4*(V_r_x**2+V_r_y**2)/(1-(V_r_x**2+V_r_y**2)**2)
    diff_phi = k * (diff_Z_p + (d_mea-d_ref))
    I_beat = I_0*(1+np.cos(diff_phi))
#     I_beat = I_beat.astype(np.int)
    
    '''
        Wave Generating (2nd way)
    '''
    D_d = D + X * (V_m_x - V_r_x) + Y * (V_m_y - V_r_y)
    L_d = L + X * V_r_x + Y * V_r_y
    diff_L_d_2 = D_d + (L_d + D_d) * (1 - (V_m_x**2 + V_m_y**2)) / (1 + (V_m_x**2 + V_m_y**2)) - L_d * (1 - (V_r_x**2 + V_r_y**2)) / (1 + (V_r_x**2 + V_r_y**2))
    Z = I_0 * (1 + np.cos(2 * np.pi * diff_L_d_2 / Lamda))
    
    return (I_beat) * window_2d + 12






# im = plt.imshow(f(X, Y),cmap='gray', animated=True)
# plt.colorbar(im, fraction=0.046, pad=0.04)
# plt.show()


file = open(np_16line_name,'ab')
img = f(X, Y)
# lines = img[hor_lines]
# lines = lines.astype(int)
'''
    Noise (same for each line)
'''
# for i in range(5000):
#     lines = img[hor_lines].astype(int)
#     time_stamp = time.time()
#     multiline_noise = np.array([np.random.normal(2.43, 0.8, 1280)] * 16)
#     multiline_noise = multiline_noise.round().astype(int)
#     lines = lines + multiline_noise
#     line = np.array([lines, time_stamp])
#     np.save(file, line, allow_pickle=True)
'''
    Noise (real)
'''
for i in range(5000):
    lines = img[hor_lines]
    time_stamp = time.time()
#     multiline_noise = np.array([np.random.normal(2.43, 0.8, 1280)] * 16)
#     multiline_noise = multiline_noise.round().astype(int)
    multiline_noise = np.empty(shape=(16,1280))
    for j in range(16):
        line_noise = np.array([np.random.normal(2.43, 2.6, 1280)])
        multiline_noise[j] = line_noise
    lines = lines + multiline_noise
    lines = np.array(lines).round().astype(int)
    line = np.array([lines, time_stamp])
    np.save(file, line, allow_pickle=True)

'''
    Withou Noise
'''
# for i in range(5000):
#     lines = img[hor_lines].astype(int)
#     time_stamp = time.time()
#     multiline_noise = np.array([np.random.normal(2.43, 0.8, 1280)] * 16)
#     multiline_noise = multiline_noise.round().astype(int)
#     lines = lines + multiline_noise
#     line = np.array([lines, time_stamp])
#     np.save(file, line, allow_pickle=True)


# im = plt.imshow(lines,cmap='gray', animated=True)
# plt.colorbar(im, fraction=0.046, pad=0.04)
# plt.show()



